#include "stdafx.h"
#include "cvxQuadTree.h"

using cvx::Bw32FImage;
using cvx::Rgb8UImage;

// #define QT_N_LEVELS    11
// #define QT_ROOT_LEVEL  (QT_N_LEVELS -1)
// #define QT_MAX_VAL     (1<<(QT_ROOT_LEVEL))

/************************************************************************/
/*   
	QtNode
*/
/************************************************************************/

QtNode::QtNode()
{
	_xLocCode = 0;
	_yLocCode = 0;
	_level  = 0;
	_parent = 0;
	_children[0] = NULL;
	_children[1] = NULL;
	_children[2] = NULL;
	_children[3] = NULL;
	_data = 0;
}
void QtNode::createChildrenNode()
{	
	for (int i = 0; i<4; ++i)
	{
		_children[i] = new QtNode;
	}
}

bool QtNode::isLeaf()const
{
	return _children[0] == NULL &&
		   _children[1] == NULL &&
		   _children[2] == NULL &&
		   _children[3] == NULL; 

}


void QtNode::set(bool xRight, bool yBottom, QtNode *parent)
{
	assert(parent);
	assert(parent->_level >= 1);
	_level = parent->_level - 1;
	if (xRight){			
		_xLocCode = parent->_xLocCode + (1 << _level) ;
	}
	else{
		_xLocCode = parent->_xLocCode;
	}	
	if (yBottom){
		_yLocCode = parent->_yLocCode + (1 << _level);
	}
	else {
		_yLocCode = parent->_yLocCode;
	}	
	_parent = parent;
	_data = NULL;
}

/************************************************************************/
/*      
	CvxQuadTree
*/
/************************************************************************/

CvxQuadTree::CvxQuadTree(unsigned int levels)
			:m_N_LEVELS(levels),m_ROOT_LEVEL(levels-1),m_MAX_VAL(1<<(levels-1)), m_root(NULL), m_nodeNum(0)
{
}


CvxQuadTree::~CvxQuadTree()
{

}

void CvxQuadTree::create(QtNode * node)
{
	if (node == NULL)
	{
		m_root = new QtNode;
		m_root->createChildrenNode();
		m_root->_level = m_ROOT_LEVEL;
		m_root->_children[0]->set(false, false, m_root);
		m_root->_children[1]->set(true, false, m_root);
		m_root->_children[2]->set(false, true, m_root);
		m_root->_children[3]->set(true, true, m_root);
		for (int i = 0; i<4; ++i)
		{
			if(m_root->_children[i])
			{
				create(m_root->_children[i]);
			}			
		}
	}
	else if (node->_level > 0)
	{
		int sz = 1<<node->_level;
		node->createChildrenNode();
		node->_children[0]->set(false, false, node);
		node->_children[1]->set(true, false, node);
		node->_children[2]->set(false, true, node);
		node->_children[3]->set(true, true, node);
		for (int i = 0; i<4; ++i)
		{		
			if(node->_children[i])
			{
				create(node->_children[i]);
			}
		}
	}	
}

void CvxQuadTree::balance()
{
	bool changed = true;
	while(changed)
	{
		changed = false;
		ConstructRestrictedQuadTree(NULL, changed);
	}
}
void CvxQuadTree::cloneTree(const QtNode * orgNode, QtNode *dNode)
{
	assert(dNode);

	if (orgNode->isLeaf())
	{
		if (orgNode->_data)
		{
			QtData *pData = new QtData;
			pData->copy((QtData *)orgNode->_data);
			dNode->_data = (void *)pData;
		}		
	}
	else 
	{
		dNode->createChildrenNode();
		dNode->_children[0]->set(false, false, dNode);
		dNode->_children[1]->set(true, false, dNode);
		dNode->_children[2]->set(false, true, dNode);
		dNode->_children[3]->set(true, true, dNode);
		for (int i = 0; i<4; ++i)
		{
			if (orgNode->_children[i])
			{
				cloneTree(orgNode->_children[i], dNode->_children[i]);
			}
			else
			{
				delete dNode->_children[i];
				dNode->_children[i] = NULL;
			}			
		}
	}
}


QtNode *CvxQuadTree::LocateNode (int x, int y)
{
	if ( x>=m_MAX_VAL || y >= m_MAX_VAL) return NULL;

	unsigned int level = m_ROOT_LEVEL - 1;
	QtNode *pNode = m_root;
	TraverseToPosition(pNode, level, x, y);
	return pNode;
}

QtNode *CvxQuadTree::LocateLeftNeighbor(QtNode *cell)
{
	//----No left neighbor if this is the left side of the quadtree
	if (cell->_xLocCode == 0) return NULL;
	else {
		//----Get cell's x and y locational codes and the x locational code of the
		//----cell's smallest possible left neighbor
		unsigned int xLocCode = cell->_xLocCode;
		unsigned int yLocCode = cell->_yLocCode;
		unsigned int xLeftLocCode = xLocCode - 0x00000001;

		//----Determine the smallest common ancestor of the cell and the cell's 
		//----smallest possible left neighbor
		unsigned int cellLevel, nextLevel;
		unsigned int diff = xLocCode ^ xLeftLocCode;
		QtNode *pCell = cell;    //command ancestor node
		cellLevel = nextLevel = cell->_level;
		GetCommonAncestor(pCell, nextLevel, diff);

		//----Start from the smallest common ancestor and follow the branching 
		//----patterns of the locational codes downward to the smallest left
		//----neighbor of size greater than or equal to cell
		nextLevel--;
		TraverseToLevel(pCell, nextLevel, xLeftLocCode, yLocCode, cellLevel);
		return(pCell);
	}
}

QtNode *CvxQuadTree::LocateBottomNeighbor(QtNode *cell)
{
	//----No bottom neighbor if this is the bottom side of the quadtree
	if (cell->_yLocCode == 0) return NULL;
	else {
		//----Get cell's x and y locational codes and the y locational code of the
		//----cell's smallest possible left neighbor
		unsigned int xLocCode = cell->_xLocCode;
		unsigned int yLocCode = cell->_yLocCode;
		unsigned int yBottomLocCode = yLocCode - 0x00000001;

		//----Determine the smallest common ancestor of the cell and the cell's 
		//----smallest possible bottom neighbor
		unsigned int cellLevel, nextLevel;
		unsigned int diff = yLocCode ^ yBottomLocCode;
		QtNode *pCell = cell;
		cellLevel = nextLevel = cell->_level;
		GetCommonAncestor(pCell, nextLevel, diff);

		//----Start from the smallest common ancestor and follow the branching 
		//----patterns of the locational codes downward to the smallest bottom
		//----neighbor of size greater than or equal to cell
		nextLevel--;
		TraverseToLevel(pCell, nextLevel, xLocCode, yBottomLocCode, cellLevel);
		return(pCell);
	}
}

QtNode *CvxQuadTree::LocateRightNeighbor (QtNode *cell)
{
	//----No right neighbor if this is the right side of the quadtree
	unsigned int binaryCellSize = 1 << cell->_level;
	if ((cell->_xLocCode + binaryCellSize) >= (1 << m_ROOT_LEVEL)) return NULL;
	else {
		//----Get cell's x and y locational codes and the x locational code of the
		//----cell's right neighbors
		unsigned int xLocCode = cell->_xLocCode;
		unsigned int yLocCode = cell->_yLocCode;
		unsigned int xRightLocCode = xLocCode + binaryCellSize;


		//----Determine the smallest common ancestor of the cell and the cell's 
		//----right neighbors 
		unsigned int cellLevel, nextLevel;
		unsigned int diff = xLocCode ^ xRightLocCode;
		QtNode *pCell = cell;
		cellLevel = nextLevel = cell->_level;
		GetCommonAncestor(pCell, nextLevel, diff);	

		//----Start from the smallest common ancestor and follow the branching 
		//----patterns of the locational codes downward to the smallest right
		//----neighbor of size greater than or equal to cell
		nextLevel--;
		TraverseToLevel(pCell, nextLevel, xRightLocCode, yLocCode, cellLevel);		                                                             
		return(pCell);
	}
}

QtNode *CvxQuadTree::LocateTopNeighbor(QtNode *cell)
{
	//----No Top neighbor if this is the top side of the quadtree
	unsigned int binaryCellSize = 1 << cell->_level;
	if ((cell->_yLocCode + binaryCellSize) >= (1 << m_ROOT_LEVEL)) return NULL;
	else {
		//----Get cell's x and y locational codes and the x locational code of the
		//----cell's right neighbors
		unsigned int xLocCode = cell->_xLocCode;
		unsigned int yLocCode = cell->_yLocCode;	
		unsigned int yTopLocCode = yLocCode + binaryCellSize;

		//----Determine the smallest common ancestor of the cell and the cell's 
		//----top neighbors 
		unsigned int cellLevel, nextLevel;
		unsigned int diff = yLocCode ^ yTopLocCode;

		QtNode *pCell = cell;
		cellLevel = nextLevel = cell->_level;		
		GetCommonAncestor(pCell, nextLevel, diff);	

		//----Start from the smallest common ancestor and follow the branching 
		//----patterns of the locational codes downward to the smallest top
		//----neighbor of size greater than or equal to cell
		nextLevel--;	
		TraverseToLevel(pCell, nextLevel, xLocCode, yTopLocCode, cellLevel);
		return(pCell);
	}
}
int CvxQuadTree::leftNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	return 0;
}
int CvxQuadTree::bottomNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	return 0;

}
int CvxQuadTree::rightNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	return 0;

}
int CvxQuadTree::topNeighbors(QtNode *cell, QtNode *&n1, QtNode *&n2)
{
	return 0;
}

void CvxQuadTree::ConstructRestrictedQuadTree(QtNode *node, bool& changed)
{
	//start from root
	if (node == NULL)
	{
		node = m_root;
	}
	//leaf node
	if (node->isLeaf())
	{
		unsigned int level = node->_level;
		QtNode *pLeft = LocateLeftNeighbor(node);
		if (pLeft && pLeft->_level > level + 1){
			SubdivideNode(pLeft);
			changed = true;				
		}

		QtNode *pRight = LocateRightNeighbor(node);
		if (pRight && pRight->_level > level + 1){
			SubdivideNode(pRight);
			changed = true;
		}
		QtNode *pBottom = LocateBottomNeighbor(node);
		if (pBottom && pBottom->_level > level + 1){
			SubdivideNode(pBottom);
			changed = true;			
		}

		QtNode *pTop = LocateTopNeighbor(node);		
		if(pTop && pTop->_level > level + 1){
			SubdivideNode(pTop);
			changed = true;
		}		
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				ConstructRestrictedQuadTree(node->_children[i], changed);
			}			
		}
	}
}
void CvxQuadTree::SubdivideNode(QtNode *node)
{
	if (node->_level == 0){
		return;		
	}
	assert(node->isLeaf());
	if (!node->isLeaf()){
		return;
	}

	//set four children as leaf node
	node->createChildrenNode();
	node->_children[0]->set(false, false, node);
	node->_children[1]->set(true, false, node);
	node->_children[2]->set(false, true, node);
	node->_children[3]->set(true, true, node);
}
void CvxQuadTree::leafNumber(QtNode *node, int &number)
{
	if (node == NULL)
	{
		node = m_root;
		number = 0;
		leafNumber(node, number);		
	}
	else if (node->isLeaf())
	{
		number++;
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				leafNumber(node->_children[i], number);
			}			
		}
	}
}

void CvxQuadTree::pixelLeafNumber(QtNode *node, int &number)
{
	if (node == NULL)
	{
		node = m_root;
		number = 0;
		pixelLeafNumber(node, number);		
	}
	else if (node->isLeaf())
	{
		if(node->_level == 0)
		{
			number++;
		}		
	}
	else
	{
		for (int i = 0; i<4; ++i)
		{
			if (node->_children[i] != NULL)
			{
				pixelLeafNumber(node->_children[i], number);
			}			
		}
	}

}

unsigned int CvxQuadTree::GetLevel(unsigned int length)
{
	int level = 0;
	int size = 1 << level;
	while (size < length)
	{
		level ++;
		size = 1 << level;
	}
	return level + 1;
}

//gather color, idx, shading value from neighbor leaf node
void CvxQuadTree::GatherNeighborData(QtNode *leafNode, double M[][3], vector<unsigned int> &idces, 
							   vector<double> &shadingVVec, vector<unsigned int> &szVec, bool isNodeIdx)
{
	assert(leafNode);
	assert(leafNode->isLeaf());
	assert(idces.size() == shadingVVec.size());

	int num = idces.size();
	QtData *pdata = (QtData*) leafNode->_data;
	if(pdata)
	{
		M[num][0] = pdata->_r;
		M[num][1] = pdata->_g;
		M[num][2] = pdata->_b;
	}
	else
	{
		cout<<"data error"<<endl;
	}

	if (isNodeIdx)
	{
		idces.push_back(pdata->_nodeIdx);
	}
	else
	{
		idces.push_back(pdata->_idx);  //pdata->_idx
	}
	shadingVVec.push_back(pdata->_s);
}

//gather color, idx, shading value leaf node
void CvxQuadTree::GatherData(CvxQuadTree * const pTree, QtNode *cNode, double M[][3], 
					   vector<unsigned int> &idxVec, vector<double> &shadingVVec, vector<unsigned int> &szVec, bool isNodeIdx)
{
	assert(pTree);
	assert(cNode);

	QtNode *n1, *n2;
	n1 = NULL, n2 = NULL;
	pTree->leftNeighbors(cNode, n1, n2);
	if (n1 != NULL) CvxQuadTree::GatherNeighborData(n1, M, idxVec, shadingVVec, szVec, isNodeIdx);	
	if (n2 != NULL) CvxQuadTree::GatherNeighborData(n2, M, idxVec, shadingVVec, szVec, isNodeIdx);
	
	n1 = NULL, n2 = NULL;
	pTree->rightNeighbors(cNode, n1, n2);
	
	if (n1 != NULL) CvxQuadTree::GatherNeighborData(n1, M, idxVec, shadingVVec, szVec, isNodeIdx);	
	if (n2 != NULL) CvxQuadTree::GatherNeighborData(n2, M, idxVec, shadingVVec, szVec, isNodeIdx);

	n1 = NULL, n2 = NULL;
	pTree->bottomNeighbors(cNode, n1, n2);
	if (n1 != NULL) CvxQuadTree::GatherNeighborData(n1, M, idxVec, shadingVVec, szVec, isNodeIdx);	
	if (n2 != NULL) CvxQuadTree::GatherNeighborData(n2, M, idxVec, shadingVVec, szVec, isNodeIdx);

	n1 = NULL, n2 = NULL;
	pTree->topNeighbors(cNode, n1, n2);
	if (n1 != NULL) CvxQuadTree::GatherNeighborData(n1, M, idxVec, shadingVVec, szVec, isNodeIdx);	
	if (n2 != NULL) CvxQuadTree::GatherNeighborData(n2, M, idxVec, shadingVVec, szVec, isNodeIdx);

	CvxQuadTree::GatherNeighborData(cNode, M, idxVec, shadingVVec, szVec, isNodeIdx);
}




